﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Threading;

using RobotCtrl;
using RobotView;
using BTServerCE;

namespace TestatHS12_2
{
    public enum LedStates
    {
        Disable = 0,
        Flashing,
        Enable
    }

    public enum CmdStates
    {
        TrackLine = 0,
        TrackTurn,
        TrackArcLeft,
        TrackArcRight,
        StartError
    }

    public partial class Form1 : Form
    {
        #region constants
        private const float defaultAcceleration = 0.3f;
        private const float defaultSpeed = 0.5f;
        private const int defaultLedFlashingTime_ms = 400;
        #endregion

        #region members
        private Robot robot;
        private CmdStates cmdCurrState;
        private LedStates ledState; // led state selector
        private Thread driveTh; // drive Thread, start after switch changes
        private Thread speedTh; // speed handler, start during initialize object
        private Thread ledTh; // led handler, start during initialize object
        private BTServer BTserver;
        private String param0;
        private String param1;
        #endregion

        #region constructor & destructor
        public Form1()
        {
            InitializeComponent();
            robot = new Robot();

            // enable power
            robot.Drive.Power = true;

            // link needed Drive components
            driveView1.Drive = robot.Drive;
            runLine1.Drive = robot.Drive;
            runTurn1.Drive = robot.Drive;
            runArcLeftRight1.Drive = robot.Drive;

            // for debuging
            consoleView1.RobotConsole = robot.RobotConsole;
            radarView1.Radar = robot.Radar;

            // register Switches
            robot.RobotConsole[Switches.Switch1].SwitchStateChanged += SwitchStateChanged;
            robot.RobotConsole[Switches.Switch2].SwitchStateChanged += SwitchStateChanged;
            robot.RobotConsole[Switches.Switch3].SwitchStateChanged += SwitchStateChanged;
            robot.RobotConsole[Switches.Switch4].SwitchStateChanged += SwitchStateChanged;

            // install bluetooth server
            // BTserver = new BTServer(true);

            // set default state and start infinite led handler
            ledTh = new Thread(LedHandler);
            ledTh.Start();

            // set default state and start infinite speed handler
            speedTh = new Thread(SpeedHandler);
            speedTh.Start();

    //        driveTh = new Thread(DriveHandler);
    //        driveTh.Start();
        }
        #endregion

        #region properties
        private float CurrentSpeed
        {
            set
            {
                runLine1.Speed = value;
                runTurn1.Speed = value;
                runArcLeftRight1.Speed = value;
            }
        }

        private float CurrentAcceleration
        {
            set
            {
                runLine1.Acceleration = value;
                runTurn1.Acceleration = value;
                runArcLeftRight1.Acceleration = value;
            }
        }
        #endregion

        #region methods

        private String[] bt_pull(int index)
        {
            String cmd = BTserver.getCommandAt(index);
            String[] tokens = cmd.Split(' ');
            return tokens;
        }

        private void LedHandler()
        {
            Leds led; // led selector
            ledState = LedStates.Disable; // start with defined state

            while (true) // infinite loop
            {
                if (CmdStates.StartError != cmdCurrState)
                {
                    led = (Leds)(int)cmdCurrState;
                }
                else
                {
                    led = Leds.Led1;
                }
                switch (ledState)
                {
                    case LedStates.Disable:
                        // disable all led's
                        robot.RobotConsole[Leds.Led1].LedEnabled = false;
                        robot.RobotConsole[Leds.Led2].LedEnabled = false;
                        robot.RobotConsole[Leds.Led3].LedEnabled = false;
                        robot.RobotConsole[Leds.Led4].LedEnabled = false;
                        Thread.Sleep(100); // we don't need cpu ressources for 100 ms
                        break;
                    case LedStates.Flashing:
                        robot.RobotConsole[led].LedEnabled = true;
                        Thread.Sleep(defaultLedFlashingTime_ms >> 1);
                        robot.RobotConsole[led].LedEnabled = false;
                        Thread.Sleep(defaultLedFlashingTime_ms >> 1);
                        break;
                    case LedStates.Enable:
                        robot.RobotConsole[led].LedEnabled = true;
                        Thread.Sleep(100); // we don't need cpu ressources for 100 ms
                        break;
                    default:
                        ledState = LedStates.Disable;
                        break;
                }
            }
        }

        private void SpeedHandler()
        {
            // start with defined properties
            CurrentAcceleration = defaultAcceleration;
            CurrentSpeed = defaultSpeed;
            while (true) // infinite loop
            {
                if (robot.Radar.Distance > 0.1f)
                { // more than 0.1 m distance
                    CurrentSpeed = defaultSpeed;
                }
                else
                {
                    // stop robot!
                    // CurrentSpeed = 0.1f;
                    robot.Drive.Stop();
                }
                Thread.Sleep(10); // sleep 10 ms
            }
        }

        private void DriveHandler()
        {
            bool bBTDataOk;
            int index;
            BTserver = new BTServer(false);
            // on start, no led activated
            ledState = LedStates.Disable;
            // wait until bluetooth is ready
            // it means, all data are received by BTServer
            // and start token is on stack
            while(!BTserver.startReached()){
                Thread.Sleep(10);
            }
            // data ready, flashing led
            ledState = LedStates.Flashing;
            bBTDataOk = true;
            index = 0;
            do
            {
                // pull from BTServer
                String[] token = bt_pull(index);
                index++;
                String cmd = "\0";
                param0 = "\0";
                param1 = "\0";
                float value1 = 0.0f; 
                float value2 = 0.0f;
                if (0 < token.Length)
                {
                    cmd = token[0];
                    if (1 < token.Length)
                    {
                        param0 = token[1];
                        value1 = (float)Convert.ToDouble(param0);
                        if (2 < token.Length)
                        {
                            param1 = token[2];
                            value2 = (float)Convert.ToDouble(param1);
                        }
                    }
                    while (!robot.Drive.Done) ; 
                    switch (cmd)
                    {
                        case "TrackLine":
                            cmdCurrState = CmdStates.TrackLine;
                            //RouteTh = new Thread(DriveHandlerTrackLine);
                            robot.Drive.RunLine(value1, runLine1.Speed, runLine1.Acceleration);
                            while (!robot.Drive.Done) ; 
                            break;
                        case "TrackTurn":
                            cmdCurrState = CmdStates.TrackTurn;
                            robot.Drive.RunTurn(value1, runTurn1.Speed, runTurn1.Acceleration);
                            while (!robot.Drive.Done) ;
                            break;
                        case "TrackArcLeft":
                            cmdCurrState = CmdStates.TrackArcLeft;
                            robot.Drive.RunArcLeft(value2, value1, runArcLeftRight1.Speed, runArcLeftRight1.Acceleration);
                            while (!robot.Drive.Done) ;
                            break;
                        case "TrackArcRight":
                            cmdCurrState = CmdStates.TrackArcRight;
                            robot.Drive.RunArcRight(value2, value1, runArcLeftRight1.Speed, runArcLeftRight1.Acceleration);
                            while (!robot.Drive.Done) ;
                            break;
                        //case "Start":
                        default:
                            /* start or error detected */
                            cmdCurrState = CmdStates.StartError;
                            bBTDataOk = false;
                            break;
                    }
                }
            } while (bBTDataOk);
            BTserver.clearList();
            ledState = LedStates.Enable;
        }

        private void DriveHandlerTrackLine()
        {
            float value = (float)Convert.ToDouble(param0);
            robot.Drive.RunLine(value, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
         }

        private void DriveHandlerTrackTurn()
        {
            float value = (float)Convert.ToDouble(param0);
            robot.Drive.RunTurn(value, runTurn1.Speed, runTurn1.Acceleration);
            while (!robot.Drive.Done) ;
          }

        private void DriveHandlerTrackArcLeft()
        {
            float value1 = (float)Convert.ToDouble(param0);
            float value2 = (float)Convert.ToDouble(param1);
            robot.Drive.RunArcLeft(value2, value1, runArcLeftRight1.Speed, runArcLeftRight1.Acceleration);
            while (!robot.Drive.Done) ;
         }

        private void DriveHandlerTrackArcRight()
        {
            float value1 = (float)Convert.ToDouble(param0);
            float value2 = (float)Convert.ToDouble(param1);
            robot.Drive.RunArcRight(value2, value1, runArcLeftRight1.Speed, runArcLeftRight1.Acceleration);
            while (!robot.Drive.Done) ;
        }

        void SwitchStateChanged(object sender, SwitchEventArgs e)
        {
            driveTh = new Thread(DriveHandler);
            driveTh.Start();
        }

        #endregion
    }
}